/*
@brief: 弹道解算 适配陈君的rm_vision
@author: CodeAlan  华南师大Vanguard战队
*/
// 近点只考虑水平方向的空气阻力

#include <math.h>
#include <stdbool.h>
#include <stdio.h>

#include "user_lib.h"
#include "SolveTrajectory.h"

#include <math.h>
#include <stdbool.h>

//-------单方向空气阻力弹道模型--------//
#define EPSILON 1e-6f

//-------------pitch轴解算------------//
#define MAX_ITERATIONS 20       // 最大迭代次数
#define ERROR_THRESHOLD 1e-5f   // 误差阈值
double DAMPING_FACTOR = 0.3f;     // 阻尼系数
#define EPSILON 1e-6f           // 浮点误差阈值

struct tar_pos tar_position[4]; // 最多只有四块装甲板
float t = 0.5f;                 // 飞行时间

//------------整体计算----------------//
#define MAX_ARMOR_NUM 4
#define SAFE_DIV(a,b) ((b) == 0 ? 0 : (a)/(b))  // 安全除法
#define CLAMP_ANGLE(ang) (fmodf((ang) + 5*PI, 2*PI) - PI)  // 角度归一化[-π, π]
#define EPSILON 1e-6f
#define DEFAULT_TIME_BIAS 0.01f  // 默认10ms偏置时间

// typedef struct {
//     float x, y, z, yaw;
// } ArmorPosition;

// ArmorPosition tar_position[MAX_ARMOR_NUM] = {0};

/**
 * @brief 单方向空气阻力弹道模型（增强错误处理版）
 * 
 * 增强功能：
 * 1. 增加参数有效性检查
 * 2. 优化数值稳定性
 * 3. 增强边界条件处理
 * 4. 增加对计算结果有效性的验证
 * 
 * @param st 弹道参数结构体（需包含空气阻力系数k）
 * @param s 目标距离（米）
 * @param v 初速度（米/秒）
 * @param angle 发射角度（弧度）
 * @return 目标点处的高度（米），无效情况返回0
 */
float monoDirectionalAirResistanceModel(struct SolveTrajectoryParams *st, float s, float v, float angle)
{
    // 基础参数有效性检查
    if (!st || v <= EPSILON || s < 0.0f) {
        return 0.0f;
    }

    // 三角函数计算
    float cos_angle = cosf(angle);
    float sin_angle = sinf(angle);
    
    // 方向有效性检查
    if (fabsf(cos_angle) < EPSILON || cos_angle <= 0.0f) {
        return 0.0f;
    }

    // 空气阻力系数检查
    float k = st->k;
    if (k < 0.0f) {
        return 0.0f;
    }

    // 核心参数计算
    float denominator = v * cos_angle;
    float t = 0.0f;

    /* 时间计算分两种情况：
     * 1. 无空气阻力情况 (k == 0)
     * 2. 有空气阻力情况 (k > 0)
     */
    if (fabsf(k) < EPSILON) { // 无空气阻力情况
        t = s / denominator;
    } else {                  // 有空气阻力情况
        float term = (k * s) / denominator;
        
        // 射程有效性检查
        if (term >= 1.0f - EPSILON) {
            return 0.0f;
        }

        // 数值稳定性保护
        float log_arg = 1.0f - term;
        if (log_arg < EPSILON) {
            return 0.0f;
        }

        // 时间计算
        float log_val = logf(log_arg);
        if (!isfinite(log_val)) {
            return 0.0f;
        }
        t = -log_val / k;
    }

    // 时间有效性验证
    if (t < EPSILON || !isfinite(t)) {
        return 0.0f;
    }

    // 高度计算
    float z = v * sin_angle * t - 0.5f * GRAVITY_S * t * t;
    
    // 最终结果验证
    return (z > EPSILON) ? z : 0.0f;
}


/**
 * @brief pitch轴解算（增强错误处理版）
 * 
 * 关键改进：
 * 1. 强化输入参数验证
 * 2. 增加中间计算保护
 * 3. 优化迭代收敛条件
 * 4. 增强数值稳定性
 * 
 * @param st 弹道参数结构体
 * @param s 目标距离（必须非负）
 * @param z 目标高度（有效数值）
 * @param v 初速度（必须为正）
 * @return 俯仰角补偿（弧度），错误时返回0.0f
 */
float pitchTrajectoryCompensation(struct SolveTrajectoryParams *st, float s, float z, float v)
{
    /*----------------------------
    参数有效性验证
    ----------------------------*/
    // 指针有效性检查
    if (!st || !isfinite(z)) {
        return 0.0f;
    }
    
    // 物理参数有效性检查
    if (s < 0.0f || v <= EPSILON) {
        return 0.0f;
    }

    // 特殊情形处理：零距离
    if (s < EPSILON) {
        return (z > EPSILON) ? atan2f(z, EPSILON) : 0.0f;
    }

    /*----------------------------
    迭代过程初始化
    ----------------------------*/
    float z_temp = z;
    float angle_pitch = 0.0f;
    float prev_error = INFINITY;  // 用于震荡检测

    /*----------------------------
    主迭代循环（带多重保护）
    ----------------------------*/
    for (int i = 0; i < MAX_ITERATIONS; i++) {
        // 中间值保护
        if (!isfinite(z_temp)) {
            break;
        }

        /*----------------------------
        角度计算层
        ----------------------------*/
        angle_pitch = atan2f(z_temp, s);
        
        // 角度有效性验证
        if (!isfinite(angle_pitch)) {
            angle_pitch = 0.0f;
            break;
        }

        /*----------------------------
        弹道模型计算层
        ----------------------------*/
        float z_actual = monoDirectionalAirResistanceModel(st, s, v, angle_pitch);
        
        // 弹道结果验证
        if (z_actual < 0.0f || isnan(z_actual) || !isfinite(z_actual)) {
            angle_pitch = 0.0f;
            break;
        }

        /*----------------------------
        误差分析层
        ----------------------------*/
        float error = z - z_actual;
        
        // 提前终止条件
        if (fabsf(error) < ERROR_THRESHOLD) {
            break;
        }

        /*----------------------------
        迭代控制层
        ----------------------------*/
        // 震荡检测（连续两次误差符号变化）
        if (i > 0 && error * prev_error < 0.0f) {
            DAMPING_FACTOR *= 0.5f;  // 动态调整阻尼系数
        }
        prev_error = error;

        // 带限幅的误差修正
        float dz = DAMPING_FACTOR * error;
        dz = fmaxf(fminf(dz, 2.0f), -2.0f);  // 限制最大步长
        
        // 更新临时高度（带有效性保护）
        z_temp += dz;
        z_temp = fmaxf(z_temp, 0.0f);  // 高度不低于地面
    }

    /*----------------------------
    最终结果验证
    ----------------------------*/
    // 最终角度合理性检查
    if (!isfinite(angle_pitch) || fabsf(angle_pitch) > M_PI_2) {
        angle_pitch = 0.0f;
    }

    return angle_pitch;
}

/**
 * @brief 自动弹道解算（增强安全版本）
 * 
 * 改进点：
 * 1. 全参数有效性验证
 * 2. 数学运算安全保护
 * 3. 中间值合理性检查
 * 4. 输出值消毒处理
 * 
 * @param st 弹道参数结构体（必须有效）
 * @param pitch 传出俯仰角（弧度）
 * @param yaw 传出偏航角（弧度）
 * @param aim_x 目标x坐标
 * @param aim_y 目标y坐标
 * @param aim_z 目标z坐标
 */
void autoSolveTrajectory(struct SolveTrajectoryParams *st, 
                        float *pitch, float *yaw,
                        float *aim_x, float *aim_y, float *aim_z) 
{
    /*-------------------------
    前置安全检查
    -------------------------*/
    // 空指针检查
    if (!st || !pitch || !yaw || !aim_x || !aim_y || !aim_z) return;
    
    // 初始化输出参数
    *pitch = *yaw = *aim_x = *aim_y = *aim_z = 0.0f;

    // 结构体数据有效性验证
    if (!isfinite(st->vis_data.xw)) st->vis_data.xw = 0.0f;
    if (!isfinite(st->vis_data.yw)) st->vis_data.yw = 0.0f;
    if (!isfinite(st->vis_data.tar_yaw)) st->vis_data.tar_yaw = 0.0f;

    /*-------------------------
    时间延迟计算（关键改进点）
    -------------------------*/
    float t = 0.0f;
    // 处理首次发弹情况：使用默认初速或上次有效值
    if (st->current_v > EPSILON) {
        t = SAFE_DIV(sqrtf((*aim_x)*(*aim_x) + (*aim_y)*(*aim_y)), st->current_v); // 需补全实际飞行时间模型
    } else {
        // 首次发弹处理策略：使用保守估计值
        t = 0.5f;  // 示例默认值，需根据实际调整
    }
    
    // 偏置时间安全处理
    float bias_time = isfinite(st->bias_time) ? 
                    fmaxf(st->bias_time, 0.0f) : 
                    DEFAULT_TIME_BIAS;
    float timeDelay = bias_time/1000.0f + t;
    timeDelay = fmaxf(timeDelay, 0.0f);  // 确保非负

    /*-------------------------
    目标运动预测（带角度约束）
    -------------------------*/
    if (isfinite(st->vis_data.v_yaw)) {
        st->vis_data.tar_yaw = CLAMP_ANGLE(
            st->vis_data.tar_yaw + 
            st->vis_data.v_yaw * timeDelay - 
            0.0005f
        );
    }

    /*-------------------------
    装甲板坐标计算（四象限保护）
    -------------------------*/
    
    int use_r1 = 1;
    for (int i=0; i<MAX_ARMOR_NUM; ++i) {
        // 角度计算
        float tmp_yaw = CLAMP_ANGLE(st->vis_data.tar_yaw + i*PI/2.0f);
        
        // 半径安全处理
        float r = use_r1 ? fmaxf(st->vis_data.r1, 0.0f) : 
                         fmaxf(st->vis_data.r2, 0.0f);
        
        // 坐标计算（带三角函数保护）
        float cos_yaw = cosf(tmp_yaw);
        float sin_yaw = sinf(tmp_yaw);
        if (!isfinite(cos_yaw)) cos_yaw = 1.0f;
        if (!isfinite(sin_yaw)) sin_yaw = 0.0f;
        
        tar_position[i].x = st->vis_data.xw - r * cos_yaw;
        tar_position[i].y = st->vis_data.yw - r * sin_yaw;
        tar_position[i].z = use_r1 ? st->vis_data.zw : 
                                 fmaxf(st->vis_data.zw + st->vis_data.dz, 0.0f);
        tar_position[i].yaw = tmp_yaw;

        // 坐标消毒
        if (!isfinite(tar_position[i].x)) tar_position[i].x = 0.0f;
        if (!isfinite(tar_position[i].y)) tar_position[i].y = 0.0f;
        if (!isfinite(tar_position[i].z)) tar_position[i].z = 0.0f;
        
        use_r1 = !use_r1;
    }

    /*-------------------------*/
    //装甲板选择策略（安全版本）
    //-------------------------*/
    int idx = 0;
    float min_angle_diff = INFINITY;
    for (int i=0; i<MAX_ARMOR_NUM; ++i) {
        float angle_diff = fabsf(CLAMP_ANGLE(*yaw - tar_position[i].yaw));
        if (angle_diff < min_angle_diff && isfinite(angle_diff)) {
            min_angle_diff = angle_diff;
            idx = i;
        }
    }
    idx = (idx >=0 && idx <4) ? idx : 0;  // 防越界

    /*-------------------------
    目标位置预测（速度安全处理）
    -------------------------*/
    float vx = isfinite(st->vis_data.vxw) ? st->vis_data.vxw : 0.0f;
    float vy = isfinite(st->vis_data.vyw) ? st->vis_data.vyw : 0.0f;
    float vz = isfinite(st->vis_data.vzw) ? st->vis_data.vzw : 0.0f;
    
    *aim_x = tar_position[idx].x + vx * timeDelay;
    *aim_y = tar_position[idx].y + vy * timeDelay;
    *aim_z = tar_position[idx].z + vz * timeDelay;

    /*-------------------------
    偏航角计算（安全版本）
    -------------------------*/
    float dx = *aim_x, dy = *aim_y;
    if (fabsf(dx) < EPSILON && fabsf(dy) < EPSILON) {
        *yaw = 0.0f;
    } else {
        *yaw = atan2f(dy, dx);
        if (!isfinite(*yaw)) *yaw = 0.0f;
        *yaw = CLAMP_ANGLE(*yaw);
    }

    /*-------------------------
    俯仰角计算（多重保护）
    -------------------------*/
    if (st->current_v > EPSILON) {
        float s = sqrtf(fmaxf(dx*dx + dy*dy, 0.0f)) - st->s_bias;
        s = fmaxf(s, 0.0f);  // 距离非负
        
        float comp_z = *aim_z + st->z_bias;
        comp_z = isfinite(comp_z) ? comp_z : 0.0f;
        
        float temp_pitch = -pitchTrajectoryCompensation(st, s, comp_z, st->current_v);
        *pitch = isfinite(temp_pitch) ? temp_pitch : 0.0f;
    } else {
        *pitch = 0.0f;  // 无效初速处理
    }

    /*-------------------------
    最终消毒处理
    -------------------------*/
    // 特殊目标ID处理
    if (st->vis_data.id == 0) {
        *pitch = *yaw = 0.0f;
    }

    // NaN过滤
    *pitch = isfinite(*pitch) ? *pitch : 0.0f;
    *yaw = isfinite(*yaw) ? CLAMP_ANGLE(*yaw) : 0.0f;
    *aim_x = isfinite(*aim_x) ? *aim_x : 0.0f;
    *aim_y = isfinite(*aim_y) ? *aim_y : 0.0f;
    *aim_z = isfinite(*aim_z) ? *aim_z : 0.0f;
}